package math2d

import (
	"context"
	"errors"
	"math"
	"strconv"

	"192.168.1.75/go-pkg/errorx"
	"192.168.1.75/go-pkg/logx"
	"github.com/RyanCarrier/dijkstra"
)

type Point struct {
	X float64 `json:"x"`
	Y float64 `json:"y"`
}

type Segment struct {
	Point1 Point
	Point2 Point
}

// 计算点到多条线段最近的点
func GetNearestPointToSegments(p Point, segments []Segment) (nearestPoint Point, nearestDistance float64) {
	for index, segment := range segments {
		if index == 0 {
			nearestPoint = GetNearestPointToSegment(p, Segment{
				segment.Point1, segment.Point2,
			})
			nearestDistance = GetPointDistance(p, nearestPoint)
		} else {
			nearestPointTemp := GetNearestPointToSegment(p, Segment{
				segment.Point1, segment.Point2,
			})
			distance := GetPointDistance(p, nearestPointTemp)
			if distance < nearestDistance {
				nearestDistance = distance
				nearestPoint = nearestPointTemp
			}
		}
	}
	return nearestPoint, nearestDistance
}

// 计算点到线段最近的点
func GetNearestPointToSegment(p Point, sg Segment) Point {
	vp := GetVerticalCoordinate(p, sg)
	// 如果垂点不在两点之间,去距离较近的p1或p2
	if !(((vp.X >= sg.Point1.X && vp.X <= sg.Point2.X) || (vp.X <= sg.Point1.X && vp.X >= sg.Point2.X)) &&
		((vp.Y >= sg.Point1.Y && vp.Y <= sg.Point2.Y) || (vp.Y <= sg.Point1.Y && vp.Y >= sg.Point2.Y))) {
		if GetPointDistance(p, sg.Point1) >= GetPointDistance(p, sg.Point2) {
			return sg.Point2
		} else {
			return sg.Point1
		}
	} else {
		return vp
	}
}

// GetNearestPointToPoints 从所有点中获取距离最近的点
func GetNearestPointToPoints(p Point, points []Point) (Point, error) {
	nearestPoint := Point{}
	nearestDistance := float64(-1)
	if len(points) == 0 {
		return Point{}, errors.New("invalid points")
	}
	if len(points) == 1 && points[0] == p {
		return Point{}, errors.New("no found")
	}
	for _, point := range points {
		if p == point {
			continue
		}
		distance := GetPointDistance(p, point)
		if nearestDistance == -1 {
			nearestPoint = point
			nearestDistance = distance
		}
		if distance < nearestDistance {
			nearestPoint = point
			nearestDistance = distance
		}
	}
	return nearestPoint, nil
}

// GetPointDistance 计算两点之间的距离
func GetPointDistance(p1 Point, p2 Point) float64 {
	return math.Sqrt((p1.X-p2.X)*(p1.X-p2.X) + (p1.Y-p2.Y)*(p1.Y-p2.Y))
}

// GetVerticalCoordinate 计算点到直线的垂点坐标
func GetVerticalCoordinate(p Point, sg Segment) Point {
	a, b, c := GetLinearEquation(Segment{sg.Point1, sg.Point2})
	return Point{
		X: (b*b*p.X - a*b*p.Y - a*c) / (a*a + b*b),
		Y: (a*a*p.Y - a*b*p.X - b*c) / (a*a + b*b),
	}
}

// GetDistancePointToLine 获取点到直线的距离
func GetDistancePointToLine(p Point, sg Segment) float64 {
	vpoint := GetVerticalCoordinate(p, sg)
	return GetPointDistance(p, vpoint)
}

// GetLinearEquation 计算线性方程的系数
func GetLinearEquation(sg Segment) (A float64, B float64, C float64) {
	p1 := sg.Point1
	p2 := sg.Point2
	if p1.X == p2.X {
		return 1, 0, -p1.X
	} else if p1.Y == p2.Y {
		return 0, 1, -p1.Y
	} else {
		return 1, (p1.X - p2.X) / (p2.Y - p1.Y), (p1.Y*(p2.X-p1.X) - p1.X*(p2.Y-p1.Y)) / (p2.Y - p1.Y)
	}
}

// GetSegmentIntersectionPoint 获取两条直线的交点
func GetSegmentIntersectionPoint(sg1 Segment, sg2 Segment) (Point, error) {
	p11 := sg1.Point1
	p12 := sg1.Point2
	p21 := sg2.Point1
	p22 := sg2.Point2
	a1, b1, c1 := GetLinearEquation(sg1)
	a2, b2, c2 := GetLinearEquation(sg2)
	if a1 == a2 && b1 == b2 && c1 == c2 {
		return Point{}, errors.New("no intersection point")
	}
	if (a1 == 0 && a2 == 0) || (b1 == 0 && b2 == 0) {
		return Point{}, errors.New("no intersection point")
	}
	if a1*b2-a2*b1 == 0 || b1*a2-b2*a1 == 0 {
		return Point{}, errors.New("no intersection point")
	}
	pointx := (b1*c2 - b2*c1) / (a1*b2 - a2*b1)
	pointy := (a1*c2 - a2*c1) / (b1*a2 - b2*a1)
	// 在两条线段上
	if ((pointx >= p11.X && pointx <= p12.X) || (pointx >= p12.X && pointx <= p11.X)) &&
		((pointy >= p11.Y && pointy <= p12.Y) || (pointy >= p12.Y && pointy <= p11.Y)) &&
		((pointx >= p21.X && pointx <= p22.X) || (pointx >= p22.X && pointx <= p21.X)) &&
		((pointy >= p21.Y && pointy <= p22.Y) || (pointy >= p22.Y && pointy <= p21.Y)) {
		return Point{pointx, pointy}, nil
		// 在一条线段上
	} else if ((pointx >= p11.X && pointx <= p12.X) || (pointx >= p12.X && pointx <= p11.X)) &&
		((pointy >= p11.Y && pointy <= p12.Y) || (pointy >= p12.Y && pointy <= p11.Y)) {
		if GetPointDistance(Point{pointx, pointy}, sg2.Point1) <= 2 ||
			GetPointDistance(Point{pointx, pointy}, sg2.Point2) <= 2 {
			return Point{pointx, pointy}, nil
		}
		// 在另一条线段上
	} else if ((pointx >= p21.X && pointx <= p22.X) || (pointx >= p22.X && pointx <= p21.X)) &&
		((pointy >= p21.Y && pointy <= p22.Y) || (pointy >= p22.Y && pointy <= p21.Y)) {
		if GetPointDistance(Point{pointx, pointy}, sg1.Point1) <= 2 ||
			GetPointDistance(Point{pointx, pointy}, sg1.Point2) <= 2 {
			return Point{pointx, pointy}, nil
		}
		// 在线段延长线上
	} else {
		if (GetPointDistance(Point{pointx, pointy}, sg2.Point1) <= 2 ||
			GetPointDistance(Point{pointx, pointy}, sg2.Point2) <= 2) &&
			(GetPointDistance(Point{pointx, pointy}, sg1.Point1) <= 2 ||
				GetPointDistance(Point{pointx, pointy}, sg1.Point2) <= 2) {
			return Point{pointx, pointy}, nil
		}
	}
	return Point{pointx, pointy}, errors.New("no intersection point")
}

// CreatePathNet 生成线段网络
func CreatePathNet(paths [][]Point) ([][]Point, error) {
	if len(paths) <= 0 {
		return nil, errors.New("path can not be empty")
	}
	// 新的路径集合，包含交点
	pathsNew := make([][]Point, len(paths))
	// 交点缓存
	pathPointTmp := make(map[int]map[int][]Point)
	// copy(pathsClone, paths)
	// 遍历路径
	for pathIndex, path := range paths {
		// 其中一条路径的各个线段
		if sgs, err := CreateSegment(path); err != nil {
			continue
		} else {
			// 逐个计算与其他路径的交点
			for i := pathIndex + 1; i < len(paths); i++ {
				// 另外一条路径的各个线段
				if sgs1, err := CreateSegment(paths[i]); err != nil {
					continue
				} else {
					// 计算线段交点
					for sgIndex, sg := range sgs {
						// 重组path-起点
						if sgIndex == 0 {
							pathsNew[pathIndex] = append(pathsNew[pathIndex], sg.Point1)
						}
						// 插入缓存的交点
						if pathMap, ok1 := pathPointTmp[pathIndex]; ok1 {
							if tmpPoints, ok2 := pathMap[sgIndex]; ok2 {
								pathsNew[pathIndex] = append(pathsNew[pathIndex], tmpPoints...)
							}
						}
						for sgIndex1, sg1 := range sgs1 {
							if point, err := GetSegmentIntersectionPoint(sg, sg1); err == nil {
								// 重组path，插入交点
								pathsNew[pathIndex] = append(pathsNew[pathIndex], point)
								// 缓存交点，再下次遍历时，直接插入
								{
									if pathPointTmp[i] == nil {
										pathPointTmp[i] = make(map[int][]Point)
									}
									pathPointTmp[i][sgIndex1] = append(pathPointTmp[i][sgIndex1], point)
								}
							}
						}
						// 重组path-终点
						pathsNew[pathIndex] = append(pathsNew[pathIndex], sg.Point2)
					}
				}
			}
			// 最后一条路径，直接添加缓存的交点
			if pathIndex == len(paths)-1 {
				for sgIndex, sg := range sgs {
					// 重组path-起点
					if sgIndex == 0 {
						pathsNew[pathIndex] = append(pathsNew[pathIndex], sg.Point1)
					}
					// 插入缓存的交点
					if pathMap, ok1 := pathPointTmp[pathIndex]; ok1 {
						if tmpPoints, ok2 := pathMap[sgIndex]; ok2 {
							pathsNew[pathIndex] = append(pathsNew[pathIndex], tmpPoints...)
						}
					}
					// 重组path-终点
					pathsNew[pathIndex] = append(pathsNew[pathIndex], sg.Point2)
				}
			}
		}
	}
	return pathsNew, nil
}

// CreateSegment 生成线段
func CreateSegment(points []Point) ([]Segment, error) {
	if len(points) < 2 {
		return nil, errors.New("points number can not be less than 2")
	}
	segments := []Segment{}
	for index := range points {
		if index == len(points)-1 {
			break
		}
		segments = append(segments, Segment{Point1: points[index], Point2: points[index+1]})
	}
	return segments, nil
}

// GetNearestDistancePath 多条path中获取最近距离最近的一条
func GetNearestDistancePath(point Point, paths [][]Point) (nearestPoint Point, nearestPathIndex int) {
	nearestDistance := float64(0)
	for index, path := range paths {
		if sgs, err := CreateSegment(path); err == nil {
			pt, distance := GetNearestPointToSegments(point, sgs)
			if index == 0 {
				nearestPathIndex = index
				nearestDistance = distance
				nearestPoint = pt
			} else {
				if distance < nearestDistance {
					nearestDistance = distance
					nearestPathIndex = index
					nearestPoint = pt
				}
			}
		}
	}
	return nearestPoint, nearestPathIndex
}

// 判断点是否在pathNet上
func IsOnPathNet(point Point, paths [][]Point) bool {
	segments := []Segment{}
	for _, path := range paths {
		if segs, err := CreateSegment(path); err == nil {
			segments = append(segments, segs...)
		}
	}
	_, nearestDistance := GetNearestPointToSegments(point, segments)
	return nearestDistance < 0.5
}

// CreatePathPointMark 创建路径标记
func CreatePathPointMark(points []Point, paths [][]Point) ([]Point, [][]Point, error) {
	if len(paths) <= 0 {
		return nil, nil, errors.New("path can not be empty")
	}
	nearestPathPoints := []Point{}
	pathsClone := make([][]Point, len(paths))
	copy(pathsClone, paths)

	for _, point := range points {
		nearestPathPoint, nearestPathIndex := GetNearestDistancePath(point, paths)
		nearestPathPointMap := make(map[Point]bool)
		for _, point := range paths[nearestPathIndex] {
			nearestPathPointMap[point] = true
		}
		if !nearestPathPointMap[nearestPathPoint] {
			pathsClone[nearestPathIndex] = append(pathsClone[nearestPathIndex], nearestPathPoint)
		}
		nearestPathPoints = append(nearestPathPoints, nearestPathPoint)
	}

	return nearestPathPoints, pathsClone, nil
}

// AbleToConnected 判断两点的可连通性
func AbleToConnected(point1 Point, point2 Point, paths [][]Point) bool {
	A, B, C := GetLinearEquation(Segment{
		Point1: point1,
		Point2: point2,
	})
	minX, maxX, minY, maxY := float64(0), float64(0), float64(0), float64(0)
	if point1.X < point2.X {
		minX = float64(int64(point1.X))
		maxX = float64(int64(point2.X))
	} else {
		maxX = float64(int64(point1.X))
		minX = float64(int64(point2.X))
	}
	if point1.Y < point2.Y {
		minY = float64(int64(point1.Y))
		maxY = float64(int64(point2.Y))
	} else {
		maxY = float64(int64(point1.Y))
		minY = float64(int64(point2.Y))
	}
	// 判断中间的点是否都在路径上
	// 平行于Y轴
	if A == 0 {
		for i := minX; i < maxX; i = i + (maxX-minX)/10 {
			testX, testY := i, point1.Y
			if !IsOnPathNet(Point{X: testX, Y: testY}, paths) {
				return false
			}
		}
		// 平行于X轴
	} else if B == 0 {
		for i := minY; i < maxY; i = i + (maxY-minY)/10 {
			testX, testY := point1.X, i
			if !IsOnPathNet(Point{X: testX, Y: testY}, paths) {
				return false
			}
		}
		// x跨度大
	} else if math.Abs(point1.X-point2.X) > math.Abs(point1.Y-point2.Y) {
		// var canvas gocv.Mat
		for i := minX; i < maxX; i = i + (maxX-minX)/10 {
			testX, testY := i, float64(0)
			testY = -(A*i + C) / B
			if !IsOnPathNet(Point{X: testX, Y: testY}, paths) {
				return false
			}
		}
		// y跨度大
	} else {
		for i := minY; i < maxY; i = i + (maxY-minY)/10 {
			testX, testY := float64(0), i
			testX = -(B*i + C) / A
			if !IsOnPathNet(Point{X: testX, Y: testY}, paths) {
				return false
			}
		}
	}
	return true
}

// GetShortestPath 获取最短路径
// - startPoint 起点
// - endPoint 终点
// - position 位置点标记【原始的】
// - paths 预设路径
func GetShortestPath(startPoint Point, endPoint Point, positions []Point, paths [][]Point) (distance int64, shortestPathPoints []Point, err error) {
	vectorMap := make(map[string]bool)
	// 记录原始位置点的序号，用于临近点的对应
	originalPositionIndexMap := make(map[Point]int)
	// 用于记录dijkstra的路径添加记录
	positionIndexMap := make(map[Point]int)
	// 用于将dijkstra的路径结果与点对应
	positionMap := make(map[int]Point)

	// 创建路径网络
	logx.Info(context.Background(), "start CreatePathNet")
	pathNet, err := CreatePathNet(paths)
	logx.Info(context.Background(), "end CreatePathNet")
	if err != nil {
		logx.Error(context.Background(), "CreatePathNet Err", logx.Err(err))
		return 0, []Point{}, errorx.New("路径网络生成失败", -1)
	}

	// 创建包含位置点的路径网络
	logx.Info(context.Background(), "start CreatePathPointMark")
	nearestPoints, pathNetWithPosition, err := CreatePathPointMark(positions, pathNet)
	logx.Info(context.Background(), "end CreatePathPointMark")
	if err != nil {
		logx.Error(context.Background(), "CreatePathPointMark Err", logx.Err(err))
		return 0, []Point{}, errorx.New("生成路径临近点失败", -1)
	}

	// 位置点索引map，并判断起点和终点是否有效
	// 计算实际路径上的起点和终点
	{
		for index, position := range positions {
			originalPositionIndexMap[position] = index
		}
		originalStartPositionIndex, ok1 := originalPositionIndexMap[startPoint]
		originalEndPositionIndex, ok2 := originalPositionIndexMap[endPoint]
		if !ok1 || !ok2 {
			return 0, []Point{}, errorx.New("无效的起点或终点", -1)
		}
		startPoint = nearestPoints[originalStartPositionIndex]
		endPoint = nearestPoints[originalEndPositionIndex]
	}
	// 初始化dijkstra的路径数据
	graph := dijkstra.NewGraph()
	for _, path := range pathNetWithPosition {
		for _, point := range path {
			for _, anotherPoint := range path {
				if point == anotherPoint {
					continue
				} else {
					if !AbleToConnected(anotherPoint, point, paths) {
						continue
					}
					// 新增位置，排除已经加过的
					if _, ok := positionIndexMap[point]; !ok {
						positionIndexMap[point] = len(positionIndexMap)
						positionMap[len(positionIndexMap)-1] = point
						graph.AddVertex(positionIndexMap[point])
					}
					if _, ok := positionIndexMap[anotherPoint]; !ok {
						positionIndexMap[anotherPoint] = len(positionIndexMap)
						positionMap[len(positionIndexMap)-1] = anotherPoint
						graph.AddVertex(positionIndexMap[anotherPoint])
					}
					// 新增位置间的距离，注意-这里是双向的
					if !vectorMap[strconv.Itoa(positionIndexMap[point])+"-"+strconv.Itoa(positionIndexMap[anotherPoint])] &&
						!vectorMap[strconv.Itoa(positionIndexMap[anotherPoint])+"-"+strconv.Itoa(positionIndexMap[point])] {
						graph.AddArc(
							positionIndexMap[point],
							positionIndexMap[anotherPoint],
							int64(GetPointDistance(point, anotherPoint)*1000),
						)
						graph.AddArc(
							positionIndexMap[anotherPoint],
							positionIndexMap[point],
							int64(GetPointDistance(point, anotherPoint)*1000),
						)
						vectorMap[strconv.Itoa(positionIndexMap[point])+"-"+strconv.Itoa(positionIndexMap[anotherPoint])] = true
					}
				}
			}
		}
	}
	// 获取开始和结束位置的序号
	startPointIndex := positionIndexMap[startPoint]
	endPositionIndex := positionIndexMap[endPoint]
	// 获取最短路径
	logx.Info(context.Background(), "start cal Shortest")
	if best, err := graph.Shortest(startPointIndex, endPositionIndex); err != nil {
		return 0, nil, err
	} else {
		distance = best.Distance
		for _, i := range best.Path {
			shortestPathPoints = append(shortestPathPoints, positionMap[i])
		}
		// DebugInfo(paths, pathNetWithPosition, positions, nearestPoints, shortestPathPoints)
	}
	logx.Info(context.Background(), "end cal Shortest")
	return
}

// DebugInfo 用显示调试信息
// 注意仅在开发调试时候使用，不提交，要依赖opencv4的
// func DebugInfo(paths [][]Point, pathNet [][]Point, positions []Point, nearestPoints []Point, shortestPathPoints []Point) {
// 	// 显示路径
// 	canvas := gocv.NewMatWithSize(2000, 2000, gocv.MatTypeCV8UC3)
// 	red := color.RGBA{255, 0, 0, 0}
// 	for _, path := range paths {
// 		for index := range path {
// 			if index < len(path)-1 {
// 				gocv.Line(
// 					&canvas,
// 					image.Point{X: int(path[index].X), Y: int(path[index].Y)},
// 					image.Point{X: int(path[index+1].X), Y: int(path[index+1].Y)},
// 					red,
// 					1,
// 				)
// 			}
// 		}
// 	}
// 	gocv.IMWrite("path.png", canvas)
// 	// 显示路径网络
// 	white := color.RGBA{255, 255, 255, 0}
// 	for _, path := range pathNet {
// 		for index, position := range path {
// 			blue := color.RGBA{0, 0, 255, 0}
// 			// gocv.PutText(&canvas, strconv.Itoa(index), image.Pt(int(position.X), int(position.Y)), 0, 1, blue, 1)
// 			gocv.PutText(&canvas, strconv.Itoa(int(position.X)), image.Pt(int(position.X), int(position.Y)), 0, 1, blue, 1)
// 			gocv.Circle(&canvas, image.Pt(int(position.X), int(position.Y)), 10, blue, -1)

// 			if index < len(path)-1 {
// 				gocv.Line(
// 					&canvas,
// 					image.Point{X: int(path[index].X), Y: int(path[index].Y)},
// 					image.Point{X: int(path[index+1].X), Y: int(path[index+1].Y)},
// 					white,
// 					1,
// 				)
// 			}
// 		}
// 	}
// 	gocv.IMWrite("pathnet.png", canvas)
// 	// 原始位置点
// 	blue := color.RGBA{0, 0, 255, 0}
// 	for _, position := range positions {
// 		// gocv.PutText(&canvas, strconv.Itoa(index), image.Pt(int(position.X), int(position.Y)), 0, 2, blue, 1)
// 		gocv.Circle(&canvas, image.Pt(int(position.X), int(position.Y)), 10, blue, -1)
// 	}
// 	// 临近位置点
// 	green := color.RGBA{0, 255, 0, 0}
// 	for _, position := range nearestPoints {
// 		// gocv.PutText(&canvas, strconv.Itoa(index), image.Pt(int(position.X), int(position.Y)), 0, 2, green, 1)
// 		gocv.Circle(&canvas, image.Pt(int(position.X), int(position.Y)), 10, green, -1)
// 	}
// 	// 导航的路径
// 	for _, position := range shortestPathPoints {
// 		// gocv.PutText(&canvas, strconv.Itoa(index), image.Pt(int(position.X), int(position.Y)), 0, 2, green, 1)
// 		gocv.Circle(&canvas, image.Pt(int(position.X), int(position.Y)), 10, red, -1)
// 	}
// 	gocv.IMWrite("nav.png", canvas)
// }
